package organisms;
import field.AngleField;
import field.Bin;
import field.BinGrid;
import field.FieldInterpreter;
import geom.*;

import java.util.ArrayList;
import java.util.Iterator;

import processing.core.PApplet;
import util.Prob;



public class Tracer extends Organism{
	
	Vector heading;
	private FieldInterpreter interpreter;
	float vel;
	AngleField field;
	public Tracer(PApplet pa, AngleField field, Vector pos, Vector speed){
		super(pa, pos, field.getOffset(), field.getSize().add(field.getOffset()));
		this.field = field;
		this.vel = speed.absLength();
		this.heading = speed.normalize();
		lastSpeed = new Vector();
		setInterpreter(new Interpreter(this, field));
	}
	
	public void reset(Vector pos, Vector speed){
		super.reset(pos);
		this.vel = speed.absLength();
		this.heading = speed.normalize();
		lastSpeed = new Vector();
		setInterpreter(new Interpreter(this, field));
	}
	
	public Vector getSpeed(){
		return heading.mult(vel);
	}
	Vector lastSpeed;
	
	boolean debug = false;
	

	
	public void move(){
		if(!isAlive()) return;
		PApplet.println("pos " + pos.toString());
		ArrayList<Segment> segments = field.getSegments(getPos());
		Vector fieldValue = field.getFieldValue(getInterpreter());
		if(!isAlive()) return;
		PApplet.println("fieldval " + fieldValue);
		Vector inv = fieldValue.mult(-1f);
		if(inv.getDistance(heading) < fieldValue.getDistance(heading))
			fieldValue = inv;
		
		float strength = fieldValue.absLength();
		float closestDist = Float.POSITIVE_INFINITY;
		Iterator<Segment> it = segments.iterator();
		while(it.hasNext()){
			Segment next = it.next();
			if(this.ignoreSegment(next))
				continue;
			float dist = Geom.getDistanceFromPointToLineXY(next.startPos(), next.endPos(), getPos());
			if(dist < closestDist){
				closestDist = dist;
			}
		}
		if(closestDist == Float.POSITIVE_INFINITY) strength = 0;
		else if(closestDist == 0) strength = 1;
		else strength = (float)Math.min(0.999f, 3.5f / closestDist);
		
		float speedModifier = 1f;
		//float speedModifier = 1f - Math.min(0.7f, 5f / (float)Math.sqrt(closestDist));
		
		
		PApplet.println("fieldVal " + fieldValue.toString() + " strength " + strength);
		if(fieldValue.absLength() != 0){
			fieldValue = fieldValue.normalize().mult(strength);
			//speedModifier = 1f / strength;
		}
		//PApplet.println("heading " + heading);
		
		Vector speed = heading.mult(1 - Math.min(1,strength)).add(fieldValue).mult(speedModifier);
		PApplet.println("speed " + speed.toString());
		boolean toggle = false;
		//if(prevPos != null){
		/*
		Iterator<Segment> its = segments.iterator();
		while(its.hasNext()){
			Segment s = its.next();
			Vector at = null;
			if(!s.getParent().getParent().equals(this)){
				if(s.intersects(prevPos, getPos())){
					if(((Tracer)s.getParent().getParent()).draw)
						at = s.intersectsAt(prevPos, pos);
				}
			}
			if(at != null){
				if(draw){
					if(prevPos != null)pa.line(prevPos.x, prevPos.y, at.x, at.y);
				}
				//draw = !draw;
				//toggle = true;
			}
		}
		}
		*/
		setPos(pos.add(speed.mult(vel)));
		
		if(lastSpeed.normalize().getDistance(speed.normalize()) > 1 || debug){
			PApplet.println("pos   : " + pos.toString());
			PApplet.println("lspeed: "+ lastSpeed.toString());
			PApplet.println(" speed: " + speed.toString() + " speedmod: " + speedModifier);
			PApplet.println(" field: " + fieldValue + " strength: " + strength);
			
		}
		heading = speed.normalize();
		lastSpeed = speed;
		
	}
	protected class Interpreter extends FieldInterpreter{
		float maxLength;
		public Interpreter(Organism parent, AngleField field) {
			super(parent, field);
			maxLength = field.getBinSize().absLength();
		}

		

		@Override
		public Vector process(Segment seg, ArrayList<Segment> segments) {
			if(!parent.ignoreSegment(seg)){
				
				float dist = Geom.getDistanceFromPointToLineXY(seg.startPos(), seg.endPos(), pos) - 1f;
				//float scaled = sigE(1 / dist, 128f);
				Vector head = seg.relative();
				if(head.absLength() > Math.sqrt(maxLength)){
					head.scaleLengthTo((float)Math.sqrt(maxLength));
				}
				//head.a = head.a % (float)Math.PI;
				return head.mult(Math.max(1f / (float)Math.pow(Math.E, dist/8f), 0.0001f));	
			}
			return new Vector();
			
		}
		@Override
		public Vector postProcess(Vector fieldValue, ArrayList<Segment> segments) {
			//PApplet.println("fval 1: " + fieldValue);
			if(fieldValue.absLength() == 0) return fieldValue;
			fieldValue =  fieldValue.div((segments.size() + 1) / 16f);
			
			
			PolarVector p = fieldValue.getPolar();
			//PApplet.println("angle " + p.a);
			p.a = p.a % 2f;
			p.e = p.e % 2f;
			//PApplet.println("angle " + p.a);
			fieldValue = p.getCartesian();
			//PApplet.println("fval 2: " + fieldValue);
			
			return fieldValue;
		}
	}
	
	public Vector fieldAngle(ArrayList<Segment> segments){
		Vector val = new Vector();
		
		Iterator<Segment> il = segments.iterator();
		while(il.hasNext()){
			Segment next = il.next();
			if(!this.ignoreSegment(next)){
			
				float dist = Geom.getDistanceFromPointToLineXY(next.startPos(), next.endPos(), pos);
				float scaled = sigE(1 / dist, 128f);
				PolarVector head = next.relative().normalize().getPolar();
				//head.a = head.a % (float)Math.PI;
				val = val.add(head.getCartesian().mult(1 / (dist)));
			}
		}
		//val.x = 1f-sigE(val.x, 5);
		//val.y = 1f-sigE(val.y, 5);
		//PApplet.println(field.getSegments().size());
		
		Vector fieldValue =  val.div((field.getSegments().size() + 1) / 16f);
		Vector inv = fieldValue.mult(-1f);
		if(inv.getDistance(heading) < fieldValue.getDistance(heading))
			fieldValue = inv;
		return fieldValue;
	}
	
	public float sigmoid(float v){
		return 1f / (1f + (float)Math.pow(Math.E, v));
	}
	
	public float sigE(float v, float div){
		return 1f / (float)Math.pow(Math.E, v/div);
	}
	
	public void draw(){
		pa.noFill();
		
		pa.beginShape();
		Iterator<Segment> it = getTrace().getSegments().iterator();
		while(it.hasNext()){
			Vector p = it.next().startPos();
			pa.vertex(p.x, p.y);
		}
		if(getTrace().getLastSegment() != null){
			pa.vertex(trace.getLastSegment().startPos().x, trace.getLastSegment().startPos().y);
			pa.vertex(trace.getLastSegment().endPos().x, trace.getLastSegment().endPos().y);
		}
		pa.endShape();
		
		
		
	}


	public Vector getFieldValue(Segment seg) {
		// TODO Auto-generated method stub
		return null;
	}
	public void setPos(Vector v){
		setPos(v, true);
	}
	public void setPos(Vector v, boolean addToField){
		
		int segments = getTrace().getSegments().size();
		
		super.setPos(v);
		//PApplet.println("tracer.setpos before afgter: " + segments + " / " + getTrace().getSegments().size());
		if(addToField && getTrace().getSegments().size() > segments){
			field.add(getTrace().getSegments().get(getTrace().getSegments().size()-1));
		}
	}

	public void setInterpreter(FieldInterpreter interpreter) {
		this.interpreter = interpreter;
	}

	public FieldInterpreter getInterpreter() {
		return interpreter;
	}
	
}
